#include "can_common.h"
#include "damiao.h"
#include "dji.h"
#include "imu.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robot_cmd.h"
#include "rtthread.h"
#include "rttypes.h"
#include "robotdef.h"


#define LOG_TAG              "gimbal"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
DJIMotor_t *yaw_motor ={0};
DMMOTOR_t *pitch_motor ={0};


void gimbal_init(void)
{
    // YAW
    Motor_Init_Config_s yaw_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_YAW,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 1,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
            .can_handle = &can2_bus,
            .tx_id = 1,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 8, // 8
                .Ki = 0,
                .Kd = 2,
                .DeadBand = 0.1,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 200,
                .MaxOut = 1500,
            },
            .speed_PID = {
                .Kp = 50,  // 50
                .Ki = 200, // 200
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .MaxOut = 30000,
            },
            .other_angle_feedback_ptr = &INS.YawTotalAngle,
            .other_speed_feedback_ptr = &INS.Gyro[2],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = GM6020};
    // PITCH
    Motor_Init_Config_s pitch_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_PITCH,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 2,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
            .can_handle = &can2_bus,
            .tx_id = 0X23,
            .rx_id = 0X206,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.1, // 8
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0.01,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,
                .MaxOut = 500,
            },
            .speed_PID = {
                .Kp = 1,  // 50
                .Ki = 100, // 200
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .MaxOut = 20000,
            },
            .other_angle_feedback_ptr = &INS.Pitch,
            .other_speed_feedback_ptr = &INS.Gyro[0],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = DM4310};
    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
    yaw_motor = DJIMotorInit(&yaw_config);
    #ifdef BOARD_SMALL_YAW_1
        pitch_motor = DMMotorInit(&pitch_config,POS_MODE);
    #endif 
    #ifdef BOARD_SMALL_YAW_2
        pitch_motor = DMMotorInit(&pitch_config,POS_MODE);
    #endif 
    

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

}

static void gimbal_thread_entry(void *parameter)
{   
    rt_thread_mdelay(2000);
    gimbal_init();
    while (1)
    {
        if (SubGetMessage(gimbal_sub, &gimbal_cmd_recv)!=0)
        {
            if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
            {
                DJIMotorStop(yaw_motor);
                //DMMotorStop(pitch_motor);
                //aRGB_led_show(LED_Blue);
            }
            else if (gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE)
            {
                DJIMotorEnable(yaw_motor);
                //VAL_LIMIT(tmp, YAW_MIN_ANGLE_GYRO + centeroffset, YAW_MAX_ANGLE_GYRO + centeroffset);
                //VAL_LIMIT(gimbal_cmd_recv.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
                DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
            }
        }else{
            DJIMotorStop(yaw_motor);
        }

        // 设置反馈数据,主要是imu和yaw的ecd
        if (offline_manage[OFFLINE_GIMBAL_YAW].online_state ==STATE_ONLINE)
        {
            gimbal_feedback_data.gimbal_ins.Roll =INS.Roll;
            gimbal_feedback_data.gimbal_ins.Pitch =INS.Pitch;
            gimbal_feedback_data.gimbal_ins.Yaw =INS.Yaw;
            gimbal_feedback_data.gimbal_ins.YawTotalAngle =INS.YawTotalAngle;
            gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
            // 推送消息
            PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
        }
        rt_thread_mdelay(5);
    }

}
int gimbal_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("gimbal", gimbal_thread_entry, RT_NULL, 4096, 31, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("gimbal thread init success!\n");

    return ret;
}

#ifdef BOARD_SMALL_YAW_1 
INIT_APP_EXPORT(gimbal_thread_entry_init);
#endif 
#ifdef BOARD_SMALL_YAW_2
INIT_APP_EXPORT(gimbal_thread_entry_init);
#endif 
#ifdef BOARD_BIG_YAW 
INIT_APP_EXPORT(gimbal_thread_entry_init);
#endif 
